﻿//-------------------------------------------------------------------
//	Copyright (c) 2010-2014  Zhirnov Andrey
//	This file is part of the "UX Standard Lib" project.
//	See copyright notice in "UX_STL_Core.h".
//-------------------------------------------------------------------

#ifndef _UXS_QUATERNION_H
#define _UXS_QUATERNION_H

#pragma once

#include "TVec.h"
#include "TMatrix3.h"
#include "TMatrix4.h"

namespace UX_STL
{
namespace UXMath
{
	
#	define _QUAT_OPERATOR1( _op, _right, _func ) \
		Self&	operator _op##= (const _right& right)		_func \
		Self	operator _op  (const _right& right) const	{ return Self(*this) _op##= right; }

#	define _QUAT_OPERATOR2( _op ) \
		_QUAT_OPERATOR1( _op, T, { x _op##= right;  y _op##= right;  z _op##= right;  w _op##= right;  return *this; } )

#	define _QUAT_OPERATOR3( _op ) \
		_QUAT_OPERATOR1( _op, Self, { x _op##= right.x;  y _op##= right.y;  z _op##= right.z;  w _op##= right.w;  return *this; } )

#	define _QUAT_OPERATOR( _op ) \
		_QUAT_OPERATOR2( _op ); \
		_QUAT_OPERATOR3( _op )



	//
	// Quaternion
	//

	template <typename T>
	struct TQuaternion
	{
		// types
		typedef bool				_is_quaternion;
		typedef TQuaternion<T>		Self;
		typedef T					value_t;
		typedef TVec<T,3>			vec3_t;
		typedef TMatrix<T,3,3>		mat3_t;
		typedef TMatrix<T,4,4>		mat4_t;


		// variables
		T	x, y, z, w;
		

		// constructors
		TQuaternion(): x(0), y(0), z(0), w(1) {}
		TQuaternion(T X, T Y, T Z, T W): x(X), y(Y), z(Z), w(W) {}
		TQuaternion(T X, T Y, T Z);
		TQuaternion(const vec3_t &sPoint);
		TQuaternion(const mat3_t &sMatrix);
		TQuaternion(const mat4_t &sMatrix);
		TQuaternion(const T *vect);
		

		// unary operators
		Self		operator - () const;
		bool		operator ! () const;


		// binary operators
		T		&	operator [] (usize uNumb);
		const T &	operator [] (usize uNumb) const;

		bool		operator == (const Self &right)	const;
		bool		operator != (const Self &right)	const;

		Self &		operator =  (const Self &right);
		Self &		operator =  (const mat4_t &right);
		Self &		operator =  (const mat3_t &right);
		Self &		operator =  (const vec3_t &right);

		Self		operator *  (const Self &right)	const;
		vec3_t		operator *  (const vec3_t &right) const;
		Self &		operator *= (const Self &right);
		
		_QUAT_OPERATOR( +  );
		_QUAT_OPERATOR( -  );
		_QUAT_OPERATOR2( * );
		_QUAT_OPERATOR( /  );
		_QUAT_OPERATOR( %  );
		_QUAT_OPERATOR( &  );
		_QUAT_OPERATOR( |  );
		_QUAT_OPERATOR( ^  );
		_QUAT_OPERATOR( >> );
		_QUAT_OPERATOR( << );


		// methods
		T		*	 ptr()							{ return PointerCast< T >( this ); }
		const T	*	 ptr() const					{ return PointerCast< T >( this ); }

		Self &	Set(T X, T Y, T Z);
		Self &	Set(T X, T Y, T Z, T W);
		Self &	Set(const Self &quat)				{ return ( *this = quat ); }
		Self &	Set(const vec3_t &point)			{ return ( *this = point ); }
		Self &	Set(const mat3_t &sMatrix)			{ return ( *this = sMatrix ); }
		Self &	Set(const mat4_t &sMatrix)			{ return ( *this = sMatrix ); }
		
		T		Dot(const Self &right) const;

		bool	IsZero() const;

		Self	Normalized() const;
		Self &	Normalize();
		
		vec3_t	GetDirection() const;

		Self &	Inverse();
		Self	GetInverse() const;
		Self &	Slerp(Self sQ1, Self sQ2, T tInterpolate);

		Self &	FromAngleAxis(T tAngle, const vec3_t &sAxis);
		void	ToAngleAxis(T &tAngle, vec3_t &sAxis) const;
		void	ToEuler(vec3_t &sEuler) const;

		//Self & FromSpherical(const TVec<T,2> &sAngles);
		//void			 ToSpherical(TVec<T,2> &sAngles) const;

		Self &	MakeIdentity();
		Self &	RotationFromTo(const vec3_t &sFrom, const vec3_t &sTo);

		Self &	RotationX(T tAngle);
		Self &	RotationY(T tAngle);
		Self &	RotationZ(T tAngle);
		Self &	Rotation(const vec3_t &vAngle);

		template <typename T2>
		const TQuaternion<T2> Convert() const;

		template <typename B>
		const B	To() const;

		template <typename B, typename VT, usize C>
		const B	To() const;

		string	ToString()		const;
		string	ToStringFull()	const;
	};

	
	
//--------------------------- Math Func ---------------------------//

	template <typename T>
	inline TQuaternion<T> Abs(const TQuaternion<T> &val)
	{
		return TQuaternion<T>( Abs(val.x), Abs(val.y), Abs(val.z), Abs(val.w) );
	}


	template <typename T>
	inline TQuaternion<T> DTR(const TQuaternion<T> &val)
	{
		return TQuaternion<T>( DTR(val.x), DTR(val.y), DTR(val.z), DTR(val.w) );
	}


	template <typename T>
	inline TQuaternion<T> RTD(const TQuaternion<T> &val)
	{
		return TQuaternion<T>( RTD(val.x), RTD(val.y), RTD(val.z), RTD(val.w) );
	}
	

	template <typename T>
	inline bool IsZero(const TQuaternion<T>& val)
	{
		return val.IsZero();
	}

	
	template <typename T>
	inline TQuaternion<T> SetSign(const TQuaternion<T> &val, bool bSign)
	{
		return TQuaternion<T>(	SetSign( val.x, bSign ), SetSign( val.y, bSign ),
								SetSign( val.z, bSign ), SetSign( val.w, bSign ) );
	}
	

	template <typename T>
	inline TQuaternion<T> Mod(const TQuaternion<T>& left, const TQuaternion<T>& right)
	{
		return TQuaternion<T>(	Mod( left.x, right.x ), Mod( left.y, right.y ),
								Mod( left.z, right.z ), Mod( left.w, right.w ) );
	}

	
	template <typename R, typename T>
	inline R Round(const TQuaternion<T>& val)
	{
		return R(	Round<R::value_t>(val.x), Round<R::value_t>(val.y),
					Round<R::value_t>(val.z), Round<R::value_t>(val.w) );
	}

	
	template <typename T>
	inline TQuaternion<T> SafeDiv(const TQuaternion<T>& left, const TQuaternion<T>& right, const TQuaternion<T>& defVal)
	{
		return TQuaternion<T>(	SafeDiv( left.x, right.x, defVal.x ),
								SafeDiv( left.y, right.y, defVal.y ),
								SafeDiv( left.z, right.z, defVal.z ),
								SafeDiv( left.w, right.w, defVal.w ) );
	}


	
//----------------------------- TQuaternion -----------------------//

	template <typename T>
	inline TQuaternion<T>::TQuaternion(T X, T Y, T Z): x(0), y(0), z(0), w(1)
	{
		Set( X, Y, Z );
	}


	template <typename T>
	inline TQuaternion<T>::TQuaternion(const vec3_t &sPoint): x(0), y(0), z(0), w(1)
	{
		Set( sPoint.x, sPoint.y, sPoint.z );
	}


	template <typename T>
	inline TQuaternion<T>::TQuaternion(const mat3_t &sMatrix): x(0), y(0), z(0), w(1)
	{
		Set( sMatrix );
	}


	template <typename T>
	inline TQuaternion<T>::TQuaternion(const mat4_t &sMatrix): x(0), y(0), z(0), w(1)
	{
		Set( sMatrix );
	}
	

	template <typename T>
	inline TQuaternion<T>::TQuaternion(const T *vect): x(0), y(0), z(0), w(1)
	{
		ASSUME( vect != null );

		x = vect[0];
		y = vect[1];
		z = vect[3];
		w = vect[4];
	}
	
		
	template <typename T>
	inline T & TQuaternion<T>::operator [] (usize uNumb)
	{
		ASSUME( uNumb < 4 );
		return ptr()[uNumb];
	}

		
	template <typename T>
	inline const T & TQuaternion<T>::operator [] (usize uNumb) const
	{
		ASSUME( uNumb < 4 );
		return ptr()[uNumb];
	}


	template <typename T>
	inline bool TQuaternion<T>::operator == (const Self &right) const
	{
		return ( Equals( x, right.x ) and Equals( y, right.y ) and
				 Equals( z, right.z ) and Equals( w, right.w ) );
	}
	

	template <typename T>
	inline bool TQuaternion<T>::operator != (const Self &right) const
	{
		return not ( *this == right );
	}
	

	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::operator = (const vec3_t &right)
	{
		Set( right.x, right.y, right.z );
	}


	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::operator = (const Self &right)
	{
		x = right.x;
		y = right.y;
		z = right.z;
		w = right.w;
		return *this;
	}
	

	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::operator = (const mat3_t &right)
	{
		T			s		= (T)0;
		const T		trace	= right(0,0) + right(1,1) + right(2,2);
		Self &		q		= *this;

		if ( trace > (T)0 )
		{
			s = Sqrt( trace + (T)1 );
			w = s * (T)0.5;
			s = (T)0.5 / s;
			x = (right(1,2) - right(2,1)) * s;
			y = (right(2,0) - right(0,2)) * s;
			z = (right(0,1) - right(1,0)) * s;
		}
		else
		{
			const int	nxt[3] = {1,2,0};
			int			i = 0,
						j = 0,
						k = 0;

			if ( right(1,1) > right(0,0) )
				i = 1;

			if ( right(2,2) > right(1,1) )
				i = 2;

			j = nxt[i];
			k = nxt[j];
			s = Sqrt( (right(i,i) - (right(j,j) + right(k,k))) + (T)1.0 );

			q[i] = s * (T)0.5;
			s = (T)0.5 / s;
			q[3] = (right(j,k) - right(k,j)) * s;
			q[j] = (right(i,j) + right(j,i)) * s;
			q[k] = (right(i,k) + right(k,i)) * s;
		}
		return Normalize();
	}
	

	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::operator = (const mat4_t &right)
	{
		T			s		= (T)0;
		const T		trace	= right(0,0) + right(1,1) + right(2,2);
		Self &		q		= *this;

		if ( trace > (T)0 )
		{
			s = Sqrt( trace + (T)1 );
			w = s * (T)0.5;
			s = (T)0.5 / s;
			x = (right(1,2) - right(2,1)) * s;
			y = (right(2,0) - right(0,2)) * s;
			z = (right(0,1) - right(1,0)) * s;
		}
		else
		{
			const int	nxt[3] = {1,2,0};
			int			i = 0,
						j = 0,
						k = 0;

			if ( right(1,1) > right(0,0) )
				i = 1;

			if ( right(2,2) > right(1,1) )
				i = 2;

			j = nxt[i];
			k = nxt[j];
			s = Sqrt( (right(i,i) - (right(j,j) + right(k,k))) + (T)1.0 );

			q[i] = s * (T)0.5;
			s = (T)0.5 / s;
			q[3] = (right(j,k) - right(k,j)) * s;
			q[j] = (right(i,j) + right(j,i)) * s;
			q[k] = (right(i,k) + right(k,i)) * s;
		}
		return Normalize();
	}
	

	template <typename T>
	inline TQuaternion<T> TQuaternion<T>::operator - () const
	{
		return GetInverse();
	}
	
		
	template <typename T>
	inline bool TQuaternion<T>::operator ! () const
	{
		return not x and not y and not z and not w; 
	}


	template <typename T>
	inline TQuaternion<T> TQuaternion<T>::operator * (const Self &right) const
	{
		Self	temp;

		temp.w = (right.w * w) - (right.x * x) - (right.y * y) - (right.z * z);
		temp.x = (right.w * x) + (right.x * w) + (right.y * z) - (right.z * y);
		temp.y = (right.w * y) + (right.y * w) + (right.z * x) - (right.x * z);
		temp.z = (right.w * z) + (right.z * w) + (right.x * y) - (right.y * x);

		return temp;
	}
	

	template <typename T>
	inline TVec<T,3> TQuaternion<T>::operator * (const vec3_t &right) const
	{
		vec3_t	uv, uuv, qvec(x, y, z);

		uv	= Cross( qvec, right );
		uuv	= Cross( qvec, uv );
		uv	*= ( (T)2.0 * w );
		uuv	*=  (T)2.0;

		return right + uv + uuv;
	}


	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::operator *= (const Self &right)
	{
		return ( *this = (*this) * right );
	}


	template <typename T>
	inline T TQuaternion<T>::Dot(const Self &right) const
	{
		return (x * right.x) + (y * right.y) + (z * right.z) + (w * right.w);
	}
	
	
	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::Set(T X, T Y, T Z)
	{
		T	sr, cr,
			sp, cp,
			sy, cy;

		SinCos( X * (T)0.5, sr, cr );
		SinCos( Y * (T)0.5, sp, cp );
		SinCos( Z * (T)0.5, sy, cy );

		const T	cpcy = cp * cy;
		const T	spcy = sp * cy;
		const T	cpsy = cp * sy;
		const T	spsy = sp * sy;

		x = sr * cpcy - cr * spsy;
		y = cr * spcy + sr * cpsy;
		z = cr * cpsy - sr * spcy;
		w = cr * cpcy + sr * spsy;

		return Normalize();
	}


	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::Set(T X, T Y, T Z, T W)
	{
		z = X;
		y = Y;
		z = Z;
		w = W;
		return *this;
	}
	

	template <typename T>
	inline bool TQuaternion<T>::IsZero() const
	{
		return ( IsZero(x) and IsZero(y) and IsZero(z) and IsZero(w) );
	}


	template <typename T>
	inline TQuaternion<T> TQuaternion<T>::Normalized() const
	{
		const T		n = x*x + y*y + z*z + w*w;

		if ( n == (T)1 )
			return *this;

		return ( (*this) * ( T(1) / Sqrt(n) ) );
	}


	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::Normalize()
	{
		const T		n = x*x + y*y + z*z + w*w;

		if ( n == (T)1 )
			return *this;

		return ( *this *= (T)1.0 / Sqrt(n) );
	}
	
		
	template <typename T>
	inline TVec<T,3> TQuaternion<T>::GetDirection() const
	{
		return vec3_t(	(T)( 2.0 * x * z + 2.0 * y * w ),
							(T)( 2.0 * z * y - 2.0 * x * w ),
							(T)( 1.0 - 2.0 * x * x - 2.0 * y * y ) );
	}


	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::Inverse()
	{
		x = -x;
		y = -y;
		z = -z;
		return *this;
	}

	
	template <typename T>
	inline TQuaternion<T> TQuaternion<T>::GetInverse() const
	{
		return Self( *this ).Inverse();
	}

	
	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::Slerp(Self sQ1, Self sQ2, T tInterpolate)
	{
		T	angle = sQ1.Dot( sQ2 );

		if ( angle < (T)0.0 )
		{
			sQ1	  *= (T) -1.0;
			angle *= (T) -1.0;
		}

		T	scale,
			invscale;

		if ( ( angle + (T)1.0 ) > (T)0.05 )
		{
			if ( ( (T)1.0 - angle ) >= (T)0.05 )
			{
				const T	theta		= ACos( angle );
				const T	invsintheta	= (T)1.0 / ( Sin( theta ) );
				scale				= Sin( theta * ( (T)1.0 - tInterpolate ) ) * invsintheta;
				invscale			= Sin( theta * tInterpolate ) * invsintheta;
			}
			else
			{
				scale	 = (T)1.0 - tInterpolate;
				invscale = tInterpolate;
			}
		}
		else
		{
			sQ2.Set( -sQ1.y, sQ1.x, -sQ1.w, sQ1.z );
			scale	 = Sin( Pi<T>() * ( (T)0.5 - tInterpolate ) );
			invscale = Sin( Pi<T>() * tInterpolate );
		}

		return ( *this = ( sQ1 * scale ) + ( sQ2 * invscale ) );
	}


	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::FromAngleAxis(T tAngle, const vec3_t &sAxis)
	{
		const T	tHalfAngle	= (T)0.5 * tAngle;
		const T	tSin		= Sin( tHalfAngle );
		w = Cos( tHalfAngle );
		x = tSin * sAxis.x;
		y = tSin * sAxis.y;
		z = tSin * sAxis.z;
		return *this;
	}
	

	template <typename T>
	inline void TQuaternion<T>::ToAngleAxis(T &tAngle, vec3_t &sAxis) const
	{
		const T scale = Sqrt( x*x + y*y + z*z );

		if ( UXMath::IsZero( scale ) or w > (T)1.0 or w < (T)-1.0)
		{
			tAngle  = (T)0.0;
			sAxis.x = (T)0.0;
			sAxis.y = (T)1.0;
			sAxis.z = (T)0.0;
		}
		else
		{
			const T invscale = 1 / scale;
			tAngle  = (T)2.0 * acos(w);
			sAxis.x = x * invscale;
			sAxis.y = y * invscale;
			sAxis.z = z * invscale;
		}
	}
	

	template <typename T>
	inline void TQuaternion<T>::ToEuler(vec3_t &sEuler) const
	{
		const T		sqw = w*w;
		const T		sqx = x*x;
		const T		sqy = y*y;
		const T		sqz = z*z;

		sEuler.x = (T) atan2( T( 2.0 * (y*z +x*w) ), T(-sqx - sqy + sqz + sqw ) );
		sEuler.y = (T) asin( Clamp( T( -2.0 * (x*z - y*w) ), (T)-1.0, (T)1.0 ) );
		sEuler.z = (T) atan2( T( 2.0 * (x*y +z*w) ), T( sqx - sqy - sqz + sqw ) );
	}


	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::MakeIdentity()
	{
		w = 1.f;
		x = 0.f;
		y = 0.f;
		z = 0.f;
		return *this;
	}
	

	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::RotationFromTo(const vec3_t &sFrom, const vec3_t &sTo)
	{
		vec3_t	v0 = sFrom.Normalized(),
				v1 = sTo.Normalized();

		const T d = v0.Dot(v1);

		if ( d >= (T)1.0 )
		{
			return MakeIdentity();
		}
		else
		if ( d <= (T)-1.0 )
		{
			vec3_t	axis(1.0, 0.0, 0.0);

			axis = Cross( axis, vec3_t( x, y, z ) );

			if ( axis.Length() == (T)0.0 )
			{
				axis.Set( 0.0, 1.0, 0.0 );
				axis.Cross( vec3_t( x, y, z ) );
			}
			return FromAngleAxis( Pi<T>(), axis );
		}

		const T			s	 = Sqrt( (1+d)*2 );
		const T			invs = (T)1.0 / s;
		const vec3_t	c	 = v0.Cross(v1) * invs;
		
		x = c.x;
		y = c.y;
		z = c.z;
		w = s * (T)0.5;

		return *this;
	}


	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::RotationX(T tAngle)
	{
		const T		angle = tAngle * (T)0.5;
		x = Sin( angle );
		y = (T) 0.0;
		z = (T) 0.0;
		w = Cos( angle );
		return *this;
	}


	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::RotationY(T tAngle)
	{
		const T		angle = tAngle * (T)0.5;
		x = (T) 0.0;
		y = Sin( angle );
		z = (T) 0.0;
		w = Cos( angle );
		return *this;
	}


	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::RotationZ(T tAngle)
	{
		const T		angle = tAngle * (T)0.5;
		x = (T) 0.0;
		y = (T) 0.0;
		z = Sin( angle );
		w = Cos( angle );
		return *this;
	}
	

	template <typename T>
	inline TQuaternion<T> & TQuaternion<T>::Rotation(const vec3_t &vAngle)
	{
		*this = Self().RotationX( vAngle.x ) * Self().RotationY( vAngle.y ) * Self().RotationZ( vAngle.z );
		return *this;
	}

	
	template <typename T>
	template <typename T2>
	inline const TQuaternion<T2>  TQuaternion<T>::Convert() const
	{
		return TQuaternion<T2>( (T2)x, (T2)y, (T2)z, (T2)w );
	}
	

	template <typename T>
	template <typename B>
	inline const B  TQuaternion<T>::To() const
	{
		STATIC_ASSERT( B::_is_quaternion(true), "type is not quaternion" );

		return Convert< B::value_t >();
	}


	template <typename T>
	template <typename B, typename VT, usize C>
	inline const B  TQuaternion<T>::To() const
	{
		B				ret;
		const usize		count	= Min<usize>( 4, C );
		
		for (usize i = 0; i < count; ++i)	ret[i] = VT( (*this)[i] );
		return ret;
	}


	template <typename T>
	inline string TQuaternion<T>::ToString() const
	{
		return ( string() << x << ", " << y << ", " << z << ", " << w );
	}

	
	template <typename T>
	inline string TQuaternion<T>::ToStringFull() const
	{
		return ( string("x: ") << x << ", y: " << y << ", z: " << z << ", w: " << w );
	}


#	undef _QUAT_OPERATOR
#	undef _QUAT_OPERATOR1
#	undef _QUAT_OPERATOR2
#	undef _QUAT_OPERATOR3

//-------------------------------------------------------------------

}	// UXMath
}	// UX_STL

#endif	// _UXS_QUATERNION_H